# -*- coding: utf-8 -*-
#抽象出来的魔方模型模块
#保存当前这个魔方的各个面的位置
'''
         魔方编号
          ____
         | 1  |
     ____|____|____ ____
    | 5  | 3  | 6  | 4  |         
    |____|____|____|____|  
         | 2  |
         |____|
								 
'''
##################################################
import restoration.arm as arm
import restoration.claw as claw

#保存每个位置对应的是魔方哪一个面，魔方转动的过程中，这个列表跟随改变
cube=['U','D','F','B','L','R'];
TURN_POSITIVE=1;
TURN_NEGATIVE=0;
#记录当前哪一个手臂对应的面作为基础面
curBaseNum=1;

def initCude():
    global cube;
    cube=['U','D','F','B','L','R'];

#魔方按照1423的顺序旋转
#从R面看旋转方向
def cubeTurnBy1423(dir,angle):
    global cube;
    for i in range(0,angle//90):
        temp=cube[0];
        if dir==TURN_POSITIVE:
            cube[0]=cube[2];
            cube[2]=cube[1];
            cube[1]=cube[3];
            cube[3]=temp;
        else:
            cube[0]=cube[3];
            cube[3]=cube[1];
            cube[1]=cube[2];
            cube[2]=temp;


#魔方按照5364的顺序旋转
#从D面看旋转方向
def cubeTurnBy5364(dir,angle):
    global cube;
    for i in range(0,angle//90):
        temp=cube[4];
        if dir==TURN_NEGATIVE:
            cube[4]=cube[2];
            cube[2]=cube[5];
            cube[5]=cube[3];
            cube[3]=temp;
        else:
            cube[4]=cube[3];
            cube[3]=cube[5];
            cube[5]=cube[2];
            cube[2]=temp;

#根据这个面的字符获取这个面所在的位置编号
def getPositionBySideChar(sideChar):
    i=0;
    for item in cube:
        i+=1;
        if item==sideChar:
            return i;	

#旋转位置1到基础面
#现在有滑环可以不用考虑气管缠绕
def turnP1ToBase():
    global curBaseNum;
    claw.setClawsState(claw.CLAW_OPEN,claw.CLAW_CLOSE);
    arm.ARM_2_Turn(arm.ARM_REVERSAL,180);
    #设置1号手臂对应的面为基础面
    curBaseNum=1;

#旋转位置2到基础面
def turnP2ToBase():
    global curBaseNum;
    curBaseNum=1;

#旋转位置3到基础面
def turnP3ToBase():
    global curBaseNum;
    ##当一个爪子夹持，另外一个是张开的时候，就应该转动爪子夹持的那个手臂
    ##这样可以节约有时候爪子开合的时间,自主select应该怎么转动
    if claw.getClawState(1)==claw.CLAW_CLOSE and claw.getClawState(2)==claw.CLAW_OPEN:
        arm.ARM_1_Turn(arm.ARM_FORWARD,90);
        claw.setClawsState(claw.CLAW_OPEN,claw.CLAW_CLOSE);
        arm.ARM_1_Turn(arm.ARM_REVERSAL,90);
        curBaseNum=2;
    elif claw.getClawState(1)==claw.CLAW_OPEN and claw.getClawState(2)==claw.CLAW_CLOSE:
        arm.ARM_2_Turn(arm.ARM_REVERSAL,90);
        claw.setClawsState(claw.CLAW_CLOSE,claw.CLAW_OPEN);
        arm.ARM_2_Turn(arm.ARM_FORWARD,90);
        curBaseNum=1;
    else :
        claw.setClawsState(claw.CLAW_CLOSE,claw.CLAW_OPEN);
        arm.ARM_1_Turn(arm.ARM_FORWARD,90);
        claw.setClawsState(claw.CLAW_OPEN,claw.CLAW_CLOSE);
        arm.ARM_1_Turn(arm.ARM_REVERSAL,90);
        curBaseNum=2;


#旋转位置4到基础面
def turnP4ToBase():
    global curBaseNum;
    if claw.getClawState(1)==claw.CLAW_CLOSE and claw.getClawState(2)==claw.CLAW_OPEN:
        arm.ARM_1_Turn(arm.ARM_REVERSAL,90);
        claw.setClawsState(claw.CLAW_OPEN,claw.CLAW_CLOSE);
        arm.ARM_1_Turn(arm.ARM_FORWARD,90);
        curBaseNum=2;
    elif claw.getClawState(1)==claw.CLAW_OPEN and claw.getClawState(2)==claw.CLAW_CLOSE:
        arm.ARM_2_Turn(arm.ARM_FORWARD,90);
        claw.setClawsState(claw.CLAW_CLOSE,claw.CLAW_OPEN);
        arm.ARM_2_Turn(arm.ARM_REVERSAL,90);
        curBaseNum=1;
    else :
        claw.setClawsState(claw.CLAW_CLOSE,claw.CLAW_OPEN);
        arm.ARM_1_Turn(arm.ARM_REVERSAL,90);
        claw.setClawsState(claw.CLAW_OPEN,claw.CLAW_CLOSE);
        arm.ARM_1_Turn(arm.ARM_FORWARD,90);
        curBaseNum=2;

#旋转位置5到基础面
def turnP5ToBase():
    global curBaseNum;
    claw.setClawsState(claw.CLAW_CLOSE,claw.CLAW_OPEN);
    arm.ARM_1_Turn(arm.ARM_REVERSAL,180);
    curBaseNum=2;

#旋转位置6到基础面
def turnP6ToBase():
    global curBaseNum;
    curBaseNum=2;

#旋转基础面
def turnBase(turnDir,angle):
    #两个爪子都闭合
    claw.setClawsState(claw.CLAW_CLOSE,claw.CLAW_CLOSE);
    if curBaseNum==1:
        arm.ARM_1_Turn(turnDir,angle);
        #判断是不是90度，如果是的话，还需要进行手臂水平归位
        if angle==90:
            claw.setClawsState(claw.CLAW_OPEN,claw.CLAW_CLOSE);
            arm.ARM_1_Turn(not turnDir,angle);
    else:
        arm.ARM_2_Turn(turnDir,angle);
        #判断是不是90度，如果是的话，还需要进行手臂水平归位
        if angle==90:
            claw.setClawsState(claw.CLAW_CLOSE,claw.CLAW_OPEN);
            arm.ARM_2_Turn(not turnDir,angle);

#暴露出去使用的接口
#sideChar,turnDir,angle分别是:旋转哪一面，旋转的方向，旋转的角度
def turn(sideChar,turnDir,angle):
    #获取这个面在哪一个位置
    print(sideChar);
    position=getPositionBySideChar(sideChar);
    #旋转对应位置的面到基础面
    if position==1:
        turnP1ToBase()
    elif position==2:
        turnP2ToBase()
    elif position==3:
        turnP3ToBase()
    elif position==4:
        turnP4ToBase()
    elif position==5:
        turnP5ToBase()
    elif position==6:
        turnP6ToBase()
    else:
        print("error in getPositionBySideChar return value")
    #旋转基础面
    turnBase(turnDir,angle)

